// This file is used to control the V4L2 camera.
#include "V4L2CamCTL.h"
// 打开视频设备
V4L2CamCTL::V4L2CamCTL(const char *device_name) {
  int fd = open(device_name, O_RDWR | O_NONBLOCK, 0);
  if (fd == -1) {
    perror("Cannot open device");
    exit(EXIT_FAILURE);
  }
  this->fd = fd;
}

V4L2CamCTL::~V4L2CamCTL() {
  stop_capture();
  // 释放资源
  for (unsigned int n = 0; n < num_buffers; ++n) {
    if (munmap(buffers[n].start, buffers[n].length) == -1) {
      perror("munmap failed");
      exit(EXIT_FAILURE);
    }
  }
  close(this->fd);
  std::cout << "V4L2CamCTL closed" << std::endl;
}
int V4L2CamCTL::open_device(const char *device_name) {
  int fd = open(device_name, O_RDWR | O_NONBLOCK, 0);
  if (fd == -1) {
    perror("Cannot open device");
    return -1;
  }
  return fd;
}
// 查询视频设备的能力
int V4L2CamCTL::query_device_capabilities(){
  struct v4l2_capability cap;
  if (ioctl(fd, VIDIOC_QUERYCAP, &cap) == -1) {
    perror("VIDIOC_QUERYCAP failed");
    return -1;
  }

  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
    std::cerr << "Device is not a video capture device" << std::endl;
    return -1;
  }

  if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
    std::cerr << "Device does not support streaming I/O" << std::endl;
    return -1;
  }

  std::cout << "Device name: " << cap.driver << std::endl;
  // 查询摄像头格式
  struct v4l2_fmtdesc fmtdesc;
  memset(&fmtdesc, 0, sizeof(fmtdesc));
  fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  fmtdesc.index = 0;

  while (ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0) {
    std::cout << "Format: " << fmtdesc.description << std::endl;
    fmtdesc.index++;
  }
  // 查询摄像头分辨率
  struct v4l2_frmsizeenum frmsize;
  memset(&frmsize, 0, sizeof(frmsize));
  frmsize.index = 0;
  frmsize.pixel_format = V4L2_PIX_FMT_MJPEG;

  while (ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize) == 0) {
    std::cout << "Frame size: " << frmsize.discrete.width << "x"
              << frmsize.discrete.height << std::endl;
    frmsize.index++;
  }
  return 0;
}

// 设置视频格式
int V4L2CamCTL::set_format(unsigned int width, unsigned int height,
                           uint32_t fmt) {
  struct v4l2_format format;
  format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  format.fmt.pix.width = width;
  format.fmt.pix.height = height;
  format.fmt.pix.pixelformat = fmt;
  format.fmt.pix.field = V4L2_FIELD_NONE;

  if (ioctl(fd, VIDIOC_S_FMT, &format) == -1) {
    perror("VIDIOC_S_FMT failed");
    return -1;
  }

  std::cout << "Set format: " << width << "x" << height << " (" << fmt << ")"
            << std::endl;
  return 0;
}

// 请求视频缓冲区
int V4L2CamCTL::request_buffers(unsigned int count,
                                struct FrameBuffer *buffers) {
  struct v4l2_requestbuffers req;
  req.count = count;
  req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.memory = V4L2_MEMORY_MMAP;
  if (ioctl(fd, VIDIOC_REQBUFS, &req) == -1) {
    if (errno == EINVAL) {
      std::cerr << "Device or resource busy" << std::endl;
    } else {
      perror("VIDIOC_REQBUFS failed");
    }
    return -1;
  }
  if (req.count < count) {
    std::cerr << "Insufficient buffer memory on device" << std::endl;
    return -1;
  }
  for (unsigned int n = 0; n < req.count; ++n) {
    struct v4l2_buffer buf;
    memset(&buf, 0, sizeof(buf));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = n;
    if (ioctl(fd, VIDIOC_QUERYBUF, &buf) == -1) {
      perror("VIDIOC_QUERYBUF failed");
      return -1;
    }
    buffers[n].length = buf.length;
    buffers[n].start = mmap(nullptr /* start anywhere */, buf.length,
                            PROT_READ | PROT_WRITE /* required */,
                            MAP_SHARED /* recommended */, fd, buf.m.offset);
    std::cout << "Buffer " << n << ": length=" << buf.length
              << ", start=" << buffers[n].start << ", m.offset=" << buf.m.offset
              << std::endl;
    if (buffers[n].start == MAP_FAILED) {
      perror("mmap failed");
      exit(EXIT_FAILURE);
    }
    if (ioctl(fd, VIDIOC_QBUF, &buf) == -1) {
      perror("VIDIOC_QBUF failed");
      exit(EXIT_FAILURE);
    }
  }
  std::cout << "Requested " << req.count << " buffers" << std::endl;
  return 0;
}
// 开始流式传输
int V4L2CamCTL::start_capture() {
  enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (ioctl(fd, VIDIOC_STREAMON, &type) == -1) {
    perror("VIDIOC_STREAMON failed");
    return -1;
  }
  std::cout << "Streaming started" << std::endl;
  return 0;
}
int V4L2CamCTL::stop_capture() {
  // 停止流式传输
  v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if (ioctl(fd, VIDIOC_STREAMOFF, &type) == -1) {
    perror("VIDIOC_STREAMOFF failed");
    return -1;
  }
  return 0;
}

// 读取视频帧
int V4L2CamCTL::read_frame(cv::Mat &frame) {
  // printf("read_frame\n");
  struct v4l2_buffer buf;
  struct timeval timeout = {2, 0}; // 设置超时时间为1秒
  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  buf.memory = V4L2_MEMORY_MMAP;
  buf.index = index;
  // 使用 select 等待设备准备好提供缓冲区
  fd_set fds;
  FD_ZERO(&fds);
  FD_SET(fd, &fds);

  if (select(fd + 1, &fds, nullptr, nullptr, &timeout) == -1) {
    perror("select failed");
    return -1;
  }
  if (ioctl(fd, VIDIOC_DQBUF, &buf) == -1) {
    std::cout << "errno:" << errno << std::endl;
    if (errno == EAGAIN) {
      std::cerr << "Timed out waiting for buffer" << std::endl;
      return -1;
    } else {
      perror("VIDIOC_DQBUF failed");
      return -1;
    }
  }

  // std::cout << "Read frame at index " << index << std::endl;

  // 解码和显示 MJPEG 数据
  unsigned char *data = static_cast<unsigned char *>(buffers[index].start);
  frame =
      cv::imdecode(cv::Mat(1, buf.bytesused, CV_8UC1, data), cv::IMREAD_COLOR);
  if (ioctl(fd, VIDIOC_QBUF, &buf) == -1) {
    perror("VIDIOC_QBUF failed");
    return -1;
  }
  index++;
  if (index >= num_buffers) {
    index = 0;
  }
  return 0;
}
// 设置摄像头缩放
int V4L2CamCTL::set_camera_zoom(int zoom) {
  struct v4l2_control control;
  memset(&control, 0, sizeof(control));
  control.id = V4L2_CID_ZOOM_ABSOLUTE;
  control.value = zoom;
  if (ioctl(fd, VIDIOC_S_CTRL, &control) == -1) {
    perror("VIDIOC_S_CTRL failed");
    return -1;
  }
  std::cout << "Set zoom to " << zoom << std::endl;
  return 0;
}

// 设置曝光度
int V4L2CamCTL::set_camera_exposure(int exposure) {
  struct v4l2_queryctrl Setting;
  Setting.id = V4L2_CID_EXPOSURE_ABSOLUTE;
  int ret = ioctl(fd, VIDIOC_QUERYCTRL, &Setting);
  printf("Setting.type: %d\n", Setting.type);
  printf("Setting.flags: %d\n", Setting.flags);
  printf("Setting.default_value: %d\n", Setting.default_value);
  printf("Setting.minimum: %d\n", Setting.minimum);
  printf("Setting.maximum: %d\n", Setting.maximum);
  printf("Setting.step: %d\n", Setting.step);
  printf("Setting.name: %s\n", Setting.name);
  // 设置曝光模式
  struct v4l2_control control;
  memset(&control, 0, sizeof(control));
  control.id = V4L2_CID_EXPOSURE_AUTO;
  control.value = V4L2_EXPOSURE_MANUAL;
  if (ioctl(fd, VIDIOC_S_CTRL, &control) == -1) {
    perror("VIDIOC_S_CTRL failed");
    return -1;
  }
  std::cout << "Set exposure mode to MANUAL" << std::endl;

  // 设置曝光度
  memset(&control, 0, sizeof(control));
  control.id = V4L2_CID_EXPOSURE_ABSOLUTE;
  control.value = exposure;
  if (ioctl(fd, VIDIOC_S_CTRL, &control) == -1) {
    perror("VIDIOC_S_CTRL failed");
    return -1;
  }
  std::cout << "Set exposure to " << exposure << std::endl;
  return 0;

}
int V4L2CamCTL::set_camera_exposure_auto() {
  struct v4l2_queryctrl Setting;
  Setting.id = V4L2_CID_EXPOSURE_AUTO_PRIORITY;
  int ret = ioctl(fd, VIDIOC_QUERYCTRL, &Setting);
  printf("Setting.type: %d\n", Setting.type);
  printf("Setting.flags: %d\n", Setting.flags);
  printf("Setting.default_value: %d\n", Setting.default_value);
  printf("Setting.minimum: %d\n", Setting.minimum);
  printf("Setting.maximum: %d\n", Setting.maximum);
  printf("Setting.step: %d\n", Setting.step);
  printf("Setting.name: %s\n", Setting.name);

  // 设置自动曝光模式
  struct v4l2_control control;
  memset(&control, 0, sizeof(control));
  control.id = V4L2_CID_EXPOSURE_AUTO;
  control.value = V4L2_EXPOSURE_AUTO; // 设置为自动曝光

  if (ioctl(fd, VIDIOC_S_CTRL, &control) == -1) {
    perror("VIDIOC_S_CTRL failed to set auto exposure");
    return -1;
  }
  std::cout << "Set exposure mode to AUTO" << std::endl;

  return 0;
}
int V4L2CamCTL::set_camera_auto_focus(bool enable) 
{
 struct v4l2_control control;
  memset(&control, 0, sizeof(control));
  control.id = V4L2_CID_FOCUS_AUTO;
  control.value = enable;
  if (ioctl(fd, VIDIOC_S_CTRL, &control) == -1) {
    perror("VIDIOC_S_CTRL failed");
    exit(EXIT_FAILURE);
    return -1;
  }
  if (enable)
    std::cout << "enable focus to auto" << std::endl;
  else
    std::cout << "disable focus to auto" << std::endl;

  return 0;

}

int V4L2CamCTL::set_camera_focus(int focus_value) {
    struct v4l2_control control;
    memset(&control, 0, sizeof(control));
    // 设置手动对焦值
    control.id = V4L2_CID_FOCUS_ABSOLUTE;
    control.value = focus_value;  // 传递给函数的对焦值
    if (ioctl(fd, VIDIOC_S_CTRL, &control) == -1) {
        perror("VIDIOC_S_CTRL failed (set manual focus)");
        return -1;
    }
    std::cout << "Set focus to manual: " << focus_value << std::endl;

    return 0;
}
int V4L2CamCTL::init_device() {
  query_device_capabilities();
  set_format(640, 480, V4L2_PIX_FMT_MJPEG);
  set_camera_zoom(0);
  set_camera_auto_focus(true);
  set_camera_exposure(50);
  // set_camera_exposure_auto();
  request_buffers(num_buffers, buffers);
  start_capture();
  return 0;
}
